#include <Arduino.h>
#include <Encoder.h>

//Serial
#define CR 13
#define LF 10

//Pitch Axis
//#define P_Home_PIN 0
#define P_R_EN_PIN 1
#define P_L_EN_PIN 2
#define P_R_PWM_PIN 3
#define P_L_PWM_PIN 4
#define P_Encoder_A 5
#define P_Encoder_B 6
//#define P_R_IS_PIN A1 //Current sense
//#define P_L_IS_PIN A2 //Current sense
//#define ISThreshold 4000 //3 Volts
#define P_Mode_Idle 0
#define P_Mode_Home 1
#define P_Mode_Voltage 2
#define P_Mode_Force 3
#define P_Mode_Speed 4
#define P_Mode_Position 5
#define P_Mode_Center 6
#define P_Mode_Break 7
int P_Mode = P_Mode_Idle;
String P_Status = "P_Idle";
int P_SubStatus = 0;
#define P_HS 10 //Homing speed
Encoder P_Encoder(P_Encoder_A, P_Encoder_B);
const unsigned int P_Freqency = 6000;
long P_AP = 0; //Actual Position
long P_TP = 0; //Target Position
long P_MP = 0; //Min Position
long P_XP = 0; //Max Position
float P_P_Scaler = .006;
int P_TV = 0; //Target voltage
int P_CV = 0; //Current voltage
int P_MV = 100; //Max voltage
float P_TF = 0; //Target force in lbs
#define P_MF 8 //Max force
float P_AS = 0; //Actual Speed
float P_TS = 0; //Target Speed
float P_MS = 15; //Max Speed
bool P_BreakOn = false;
float P_LastSpeedCalcPosition=0;
unsigned long P_LastSpeedCalcTime=0;
unsigned int P_StallCount = 0;
long P_PAP = 0;

void P_Process();
void P_SetMotorVoltage(int Voltage);
void P_Break();
void P_Home();
void P_CalculateSpeed();
void P_RegulateSpeed();
void P_GotoPosition();

//Roll Axis
//#define R_Home_PIN 0
#define R_R_EN_PIN 8
#define R_L_EN_PIN 7
#define R_R_PWM_PIN 9
#define R_L_PWM_PIN 10
#define R_Encoder_A 11
#define R_Encoder_B 12
//#define R_R_IS_PIN A1 //Current sense
//#define R_L_IS_PIN A2 //Current sense
//#define ISThreshold 4000 //3 Volts
#define R_Mode_Idle 0
#define R_Mode_Home 1
#define R_Mode_Voltage 2
#define R_Mode_Force 3
#define R_Mode_Speed 4
#define R_Mode_Position 5
#define R_Mode_Center 6
#define R_Mode_Break 7
int R_Mode = R_Mode_Idle;
String R_Status = "P_Idle";
int R_SubStatus = 0;
#define R_HS 10 //Homing speed
Encoder R_Encoder(R_Encoder_A, R_Encoder_B);
const unsigned int R_Freqency = 6000;
long R_AP = 0; //Actual Position
long R_TP = 0; //Target Position
int R_TV = 0; //Target voltage
long R_MP = 0; //Min Position
long R_XP = 0; //Max Position
float R_P_Scaler = 0.0129;
int R_CV = 0; //Current voltage
int R_MV = 100; //Max voltage
float R_TF = 0; //Target force in lbs
# define R_MF 4 //Max Force
float R_AS = 0; //Actual Speed
float R_TS = 0; //Target Speed
float R_MS = 25; //Max Speed
bool R_BreakOn = false;
float R_LastSpeedCalcPosition=0;
unsigned long R_LastSpeedCalcTime=0;
unsigned int R_StallCount = 0;
long R_PAP = 0;

//Buttons
#define BO1 14
#define BO2 15
#define BO3 16
#define BO4 17
#define BI1 18
#define BI2 19
#define BI3 20
#define BI4 21
uint16_t ButtonsValues=0;
uint16_t ReportedButtonsValues=0;

void R_Process();
void R_SetMotorVoltage(int Voltage);
void R_Break();
void R_Home();
void R_CalculateSpeed();
void R_RegulateSpeed();
void R_GotoPosition();
void ReadButtons();

void ReadSerial();

void setup() {
  Serial.begin(115200);

  //Pitch
  //pinMode(P_Home_PIN, INPUT);
  pinMode(P_R_EN_PIN, OUTPUT);
  pinMode(P_L_EN_PIN, OUTPUT);
  pinMode(P_R_PWM_PIN, OUTPUT);
  pinMode(P_L_PWM_PIN, OUTPUT);

  //Roll
  //pinMode(R_Home_PIN, INPUT);
  pinMode(R_R_EN_PIN, OUTPUT);
  pinMode(R_L_EN_PIN, OUTPUT);
  pinMode(R_R_PWM_PIN, OUTPUT);
  pinMode(R_L_PWM_PIN, OUTPUT);

  //Buttons
  pinMode(BO1, OUTPUT);
  pinMode(BO2, OUTPUT);
  pinMode(BO3, OUTPUT);
  pinMode(BO4, OUTPUT);

  pinMode(BI1, INPUT_PULLUP);
  pinMode(BI2, INPUT_PULLUP);
  pinMode(BI3, INPUT_PULLUP);
  pinMode(BI4, INPUT_PULLUP); 

}

void loop() {
  ReadSerial();
  P_Process();
  R_Process();
  //ReadButtons();
}

void ReadButtons(){
  digitalWrite(BO1, LOW);
  digitalWrite(BO2, HIGH);
  digitalWrite(BO3, HIGH);
  digitalWrite(BO4, HIGH);
  delay(1);
  bitWrite(ButtonsValues, 0, !digitalRead(BI1));
  bitWrite(ButtonsValues, 1, !digitalRead(BI2));
  bitWrite(ButtonsValues, 2, !digitalRead(BI3));
  bitWrite(ButtonsValues, 3, !digitalRead(BI4));
  //PrintButtons();

  ButtonsValues = ButtonsValues << 4;
  digitalWrite(BO1, HIGH);
  digitalWrite(BO2, LOW);
  digitalWrite(BO3, HIGH);
  digitalWrite(BO4, HIGH);
  delay(1);
  bitWrite(ButtonsValues, 0, !digitalRead(BI1));
  bitWrite(ButtonsValues, 1, !digitalRead(BI2));
  bitWrite(ButtonsValues, 2, !digitalRead(BI3));
  bitWrite(ButtonsValues, 3, !digitalRead(BI4));
  //PrintButtons(); 

  ButtonsValues = ButtonsValues << 4;
  digitalWrite(BO1, HIGH);
  digitalWrite(BO2, HIGH);
  digitalWrite(BO3, LOW);
  digitalWrite(BO4, HIGH);
  delay(1);
  bitWrite(ButtonsValues, 0, !digitalRead(BI1));
  bitWrite(ButtonsValues, 1, !digitalRead(BI2));
  bitWrite(ButtonsValues, 2, !digitalRead(BI3));
  bitWrite(ButtonsValues, 3, !digitalRead(BI4));
  //PrintButtons();

  ButtonsValues = ButtonsValues << 4;
  digitalWrite(BO1, HIGH);
  digitalWrite(BO2, HIGH);
  digitalWrite(BO3, HIGH);
  digitalWrite(BO4, LOW);
  delay(1);
  bitWrite(ButtonsValues, 0, !digitalRead(BI1));
  bitWrite(ButtonsValues, 1, !digitalRead(BI2));
  bitWrite(ButtonsValues, 2, !digitalRead(BI3));
  bitWrite(ButtonsValues, 3, !digitalRead(BI4));
  //PrintButtons(); 
  if(ReportedButtonsValues != ButtonsValues){
      Serial.print("BV,");
      Serial.println(ButtonsValues);
      ReportedButtonsValues = ButtonsValues;
  }
}

void P_Process(){
  bool ReportStatus = false;
  if(P_AP != P_Encoder.read()){
    ReportStatus = true;
  }

  P_AP = P_Encoder.read();
  P_CalculateSpeed();

  if(P_Mode == P_Mode_Idle){

  }
  else if(P_Mode == P_Mode_Home){
    if(P_SubStatus == 0){
      //Serial.println("Step 0");
      P_Status = "P_Homing";
      P_SubStatus = 1;
      P_TS = -15;
      P_StallCount = 0;
      P_RegulateSpeed();
    }
    else if(P_SubStatus == 1){
      //Serial.println("Step 1");
      P_RegulateSpeed();
      if(P_AP == P_PAP){
        P_StallCount += 1;
      }
      P_PAP = P_AP;
      if(P_StallCount > 5){
        P_SetMotorVoltage(0);
        P_MP = P_AP + 2;
        P_StallCount = 0;
        P_TV = 0;
        P_TS = 15;
        P_PAP = 0;
        P_SubStatus = 2;
        P_RegulateSpeed();
      }
    }
    else if(P_SubStatus == 2){
      //Serial.println("Step 2");
      P_RegulateSpeed();
      if(P_AP == P_PAP){
        P_StallCount += 1;
      }
      P_PAP = P_AP;
      if(P_StallCount > 5){
        P_SetMotorVoltage(0);
        P_StallCount = 0;
        P_XP = P_AP - 2;
        P_TP = P_MP + ((P_XP - P_MP) / 2);
        P_P_Scaler = 2 / ((float)(P_XP - P_MP));
        //Serial.print("Scaler=");
        //Serial.println(P_P_Scaler, 4);
        P_TV = 0;
        P_TS = 0;
        P_SubStatus = 3;
      }
    }
    else if(P_SubStatus == 3){
      //Serial.println("Step 3");
      P_GotoPosition();
      if(P_Status == "P_OnTarget"){
        P_SetMotorVoltage(0);
        P_Encoder.write(0);
        P_AP = 0;
        P_TV = 0;
        P_TP = 0;
        P_Mode = P_Mode_Voltage;
        P_Status = "P_Homed";
      }
    }
  }
  else if(P_Mode == P_Mode_Voltage){
    P_Status = "P_Voltage";
    P_SetMotorVoltage(P_TV);
  }
  else if(P_Mode == P_Mode_Force){
    P_Status = "P_Force";
    P_TV = P_TF * 25;
    P_SetMotorVoltage(P_TV);
  }
  else if(P_Mode == P_Mode_Speed){

  }
  else if(P_Mode == P_Mode_Position){
      P_GotoPosition();
  }

  if(ReportStatus){
    Serial.print("PS,");
    Serial.print(P_TP * P_P_Scaler, 4);
    Serial.print(",");
    float Temp = P_AP * P_P_Scaler;
    if(Temp > 1){
      Serial.print(1.0000);
    }
    else if(Temp < -1){
      Serial.print(-1.0000);
    }
    else{
      Serial.print(P_AP * P_P_Scaler, 4);
    }
    Serial.print(",");
    Serial.print(P_TS);
    Serial.print(",");
    Serial.print(P_AS);
    Serial.print(",");
    Serial.print(P_TV);
    Serial.print(",");
    Serial.println(P_Status);
  }
}

void P_GotoPosition(){
  if((P_AP < (P_TP + 2)) && (P_AP > (P_TP - 2))){
    P_Status = "P_OnTarget";
    P_TS = 0;
    P_TV = 0;
    P_SetMotorVoltage(P_TV);
    //P_Break();
  }
  else{
    P_Status = "P_Driving";
    P_TS = (P_TP - P_AP) * .75;
    if(P_TS > P_MS){P_TS = P_MS;}
    if(P_TS < -P_MS){P_TS = -P_MS;}
    P_RegulateSpeed();
  }
}

void P_RegulateSpeed(){
    float Delta = ((P_TS - P_AS) * 1);
    if((Delta > 0) && (Delta < 1)){Delta = 1;}
    if((Delta < 0) && (Delta > -1)){Delta = -1;}
    P_TV = P_TV + Delta;
    if(P_TV > P_MV){
      P_TV = P_MV;
      //Serial.println("Pos Limit");
    }
    if(P_TV < -P_MV){
      P_TV = -P_MV;
      //Serial.println("Neg Limit");
    }
    P_SetMotorVoltage(P_TV);
}

void P_CalculateSpeed(){
    P_AS = ((P_AP - P_LastSpeedCalcPosition) / (millis() - P_LastSpeedCalcTime)) * 100;
    P_LastSpeedCalcTime = millis();
    P_LastSpeedCalcPosition = P_AP;
}

void P_Break_On(){
    analogWrite(P_R_PWM_PIN, 0);
    analogWrite(P_L_PWM_PIN, 0); 
    digitalWrite(P_R_EN_PIN, HIGH);
    digitalWrite(P_L_EN_PIN, HIGH);
    P_BreakOn = true;
}

void P_Break_Off(){
    analogWrite(P_R_PWM_PIN, 0);
    analogWrite(P_L_PWM_PIN, 0); 
    digitalWrite(P_R_EN_PIN, LOW);
    digitalWrite(P_L_EN_PIN, LOW);
    P_BreakOn = false;
}

void P_SetMotorVoltage(int Voltage){
    if(P_CV !=  Voltage){
        P_CV=Voltage;
        if(P_BreakOn){P_Break_Off();}
        if (Voltage == 0){
            digitalWrite(P_R_EN_PIN, LOW);
            digitalWrite(P_L_EN_PIN, LOW);
            analogWrite(P_R_PWM_PIN, 0);
            analogWrite(P_L_PWM_PIN, 0);
        }
        else if (Voltage > 0){
            analogWriteFrequency(P_R_PWM_PIN, P_Freqency); 
            analogWrite(P_R_PWM_PIN, Voltage);
            analogWrite(P_L_PWM_PIN, 0);
            digitalWrite(P_R_EN_PIN, HIGH);
            digitalWrite(P_L_EN_PIN, HIGH);
        }
        else if (Voltage < 0){
            analogWriteFrequency(P_L_PWM_PIN, P_Freqency); 
            analogWrite(P_R_PWM_PIN, 0);
            analogWrite(P_L_PWM_PIN, -Voltage);
            digitalWrite(P_R_EN_PIN, HIGH);
            digitalWrite(P_L_EN_PIN, HIGH);
        }
    }
}

void R_Process(){
  bool ReportStatus = false;
  if(R_AP != R_Encoder.read()){
    ReportStatus = true;
  }

  R_AP = R_Encoder.read();
  R_CalculateSpeed();

  if(R_Mode == R_Mode_Idle){

  }
  else if(R_Mode == R_Mode_Home){
    if(R_SubStatus == 0){
      //Serial.println("Step 0");
      R_Status = "R_Homing";
      R_SubStatus = 1;
      R_TS = -25;
      R_StallCount = 0;
      R_RegulateSpeed();
    }
    else if(R_SubStatus == 1){
      //Serial.println("Step 1");
      R_RegulateSpeed();
      if(R_AP == R_PAP){
        R_StallCount += 1;
      }
      R_PAP = R_AP;
      if(R_StallCount > 5){
        R_SetMotorVoltage(0);
        R_MP = R_AP + 8;
        R_StallCount = 0;
        R_TV = 0;
        R_TS = 25;
        R_PAP = 0;
        R_SubStatus = 2;
        R_RegulateSpeed();
      }
    }
    else if(R_SubStatus == 2){
      //Serial.println("Step 2");
      R_RegulateSpeed();
      if(R_AP == R_PAP){
        R_StallCount += 1;
      }
      R_PAP = R_AP;
      if(R_StallCount > 5){
        R_SetMotorVoltage(0);
        R_StallCount = 0;
        R_XP = R_AP - 8;
        R_TP = R_MP + ((R_XP - R_MP) / 2);
        R_P_Scaler = 2 / ((float)(R_XP - R_MP));
        R_TV = 0;
        R_TS = 0;
        R_SubStatus = 3;
      }
    }
    else if(R_SubStatus == 3){
      //Serial.println("Step 3");
      R_GotoPosition();
      if(R_Status == "R_OnTarget"){
        R_SetMotorVoltage(0);
        R_Encoder.write(0);
        R_AP = 0;
        R_TV = 0;
        R_TP = 0;
        R_Mode = R_Mode_Voltage;
        R_Status = "R_Homed";
      }
    }
  }
  else if(R_Mode == R_Mode_Voltage){
    R_Status = "R_Voltage";
    R_SetMotorVoltage(R_TV);
  }
  else if(R_Mode == R_Mode_Force){
    R_Status = "R_Force";
    R_TV = R_TF * 22;
    R_SetMotorVoltage(R_TV);
  }
  else if(R_Mode == R_Mode_Speed){

  }
  else if(R_Mode == R_Mode_Position){
      R_GotoPosition();
  }

  if(ReportStatus){
    Serial.print("RS,");
    Serial.print(R_TP * R_P_Scaler, 4);
    Serial.print(",");
    float Temp = R_AP * R_P_Scaler;
    if(Temp > 1){
      Serial.print(1.0000);
    }
    else if(Temp < -1){
      Serial.print(-1.0000);
    }
    else{
      Serial.print(R_AP * R_P_Scaler, 4);
    }
    Serial.print(",");
    Serial.print(R_TS);
    Serial.print(",");
    Serial.print(R_AS);
    Serial.print(",");
    Serial.print(R_TV);
    Serial.print(",");
    Serial.println(R_Status);
  }
}

void R_GotoPosition(){
  if((R_AP < (R_TP + 2)) && (R_AP > (R_TP - 2))){
    R_Status = "R_OnTarget";
    R_TS = 0;
    R_TV = 0;
    R_SetMotorVoltage(R_TV);
    //P_Break();
  }
  else{
    R_Status = "R_Driving";
    R_TS = (R_TP - R_AP) * .75; //.75
    if(R_TS > R_MS){R_TS = R_MS;}
    if(R_TS < -R_MS){R_TS = -R_MS;}
    R_RegulateSpeed();
  }
}

void R_RegulateSpeed(){
    float Delta = ((R_TS - R_AS) * .2);
    if((Delta > 0) && (Delta < 1)){Delta = 1;}
    if((Delta < 0) && (Delta > -1)){Delta = -1;}
    R_TV = R_TV + Delta;
    if(R_TV > R_MV){R_TV = R_MV;}
    if(R_TV < -R_MV){R_TV = -R_MV;}
    R_SetMotorVoltage(R_TV);
}

void R_CalculateSpeed(){
    R_AS = ((R_AP - R_LastSpeedCalcPosition) / (millis() - R_LastSpeedCalcTime)) * 100;
    R_LastSpeedCalcTime = millis();
    R_LastSpeedCalcPosition = R_AP;
}

void R_Break_On(){
    analogWrite(R_R_PWM_PIN, 0);
    analogWrite(R_L_PWM_PIN, 0); 
    digitalWrite(R_R_EN_PIN, HIGH);
    digitalWrite(R_L_EN_PIN, HIGH);
    R_BreakOn = true;
}

void R_Break_Off(){
    analogWrite(R_R_PWM_PIN, 0);
    analogWrite(R_L_PWM_PIN, 0); 
    digitalWrite(R_R_EN_PIN, LOW);
    digitalWrite(R_L_EN_PIN, LOW);
    R_BreakOn = false;
}

void R_SetMotorVoltage(int Voltage){
    if(R_CV !=  Voltage){
        R_CV=Voltage;
        if(R_BreakOn){R_Break_Off();}
        if (Voltage == 0){
            digitalWrite(R_R_EN_PIN, LOW);
            digitalWrite(R_L_EN_PIN, LOW);
            analogWrite(R_R_PWM_PIN, 0);
            analogWrite(R_L_PWM_PIN, 0);
        }
        else if (Voltage > 0){
            analogWriteFrequency(R_R_PWM_PIN, R_Freqency); 
            analogWrite(R_R_PWM_PIN, Voltage);
            analogWrite(R_L_PWM_PIN, 0);
            digitalWrite(R_R_EN_PIN, HIGH);
            digitalWrite(R_L_EN_PIN, HIGH);
        }
        else if (Voltage < 0){
            analogWriteFrequency(R_L_PWM_PIN, R_Freqency); 
            analogWrite(R_R_PWM_PIN, 0);
            analogWrite(R_L_PWM_PIN, -Voltage);
            digitalWrite(R_R_EN_PIN, HIGH);
            digitalWrite(R_L_EN_PIN, HIGH);
        }
    }
}

void ReadSerial(){
  Serial.setTimeout(50);
  String InString = Serial.readStringUntil(LF, 20);
  if(InString.startsWith("PV")){
    P_TV = InString.substring(2).toInt();
    P_Mode = P_Mode_Voltage;
    Serial.print("PV");
    Serial.println(P_TV);
  }
  if(InString.startsWith("PF")){
    P_TF = InString.substring(2).toFloat();
    if(P_TF > P_MF){P_TF = P_MF;}
    if(P_TF < -P_MF){P_TF = -P_MF;}
    P_Mode = P_Mode_Force;
    Serial.print("PF");
    Serial.println(P_TF);
  }
  else if(InString.startsWith("PH")){
    Serial.print("PH");
    P_Mode = P_Mode_Home;
    P_SubStatus = 0;
  }
  else if(InString.startsWith("PP")){
    P_TP = (1 / P_P_Scaler) * InString.substring(2).toInt();
    P_Mode = P_Mode_Position;
    Serial.print("PP");
    Serial.println(P_TP);
  }
  else if(InString.startsWith("PB")){
    P_Break_On();
    Serial.println("PB");
  }
  else if(InString.startsWith("PO")){
    P_Break_Off();
    Serial.println("PO");
  }
  else if(InString.startsWith("RV")){
    R_TV = InString.substring(2).toInt();
    R_Mode = R_Mode_Voltage;
    Serial.print("RV");
    Serial.println(R_TV);
  }
  if(InString.startsWith("RF")){
    R_TF = InString.substring(2).toFloat();
    if(R_TF > R_MF){R_TF = R_MF;}
    if(R_TF < -R_MF){R_TF = -R_MF;}
    R_Mode = R_Mode_Force;
    Serial.print("RF");
    Serial.println(R_TF);
  }
  else if(InString.startsWith("RH")){
    Serial.print("RH");
    R_Mode = R_Mode_Home;
    R_SubStatus = 0;
  }
  else if(InString.startsWith("RP")){
    R_TP = (1 / R_P_Scaler) * InString.substring(2).toInt();
    R_Mode = R_Mode_Position;
    Serial.print("RP");
    Serial.println(R_TP);
  }
  else if(InString.startsWith("RB")){
    R_Break_On();
    Serial.println("RB");
  }
  else if(InString.startsWith("RO")){
    R_Break_Off();
    Serial.println("RO");
  }
}